#include <Servo.h>

int  VarSign = 1;
int  SerIn = -1;          // incoming byte from serial RX
long VarN[9] = {0, 0, 0, 0, 0, 0, 0, 0, 0};      // number array
int  PosN = -1;       // index counter
char VarHandlerChar = '@';

#define serv_size 4
Servo serv[serv_size];

int time_out_counter=0;
#define max_count_blinks 3
int count_blinks=0; // count blink then ready COM connection

// SERIAL PARSER 
void SerialParser(void) {
  while (Serial.available()) {
    SerIn = '@';
    SerIn = Serial.read();
    if ((SerIn >= 65) && (SerIn <= 90)) {          // if ASCII 'A' - 'Z'
      VarHandlerChar = SerIn;  
      for (PosN = 0; PosN < 9; PosN++) {           // clean up
        VarN[PosN] =  0;                           // reset Var array
      }  
      PosN = -1;                              
    }
    if (SerIn == 45) {
      VarSign = -1;
    }
    if ((SerIn >= 48) && (SerIn <= 57)) {          // if ASCII numeric '0' - '9'
      VarN[PosN] = VarN[PosN] * 10 + (SerIn - 48);
    }
    if (SerIn == ' ') {                            // if ASCII " " detected
      if (PosN > -1) {
        VarN[PosN] = VarSign * VarN[PosN];           // assign sign 
        VarSign = 1;
      }
      PosN++;
    }
    if (SerIn == 33 || SerIn == 10 || SerIn == 13) {  // '!' or CR or LF ends all this spooky things ...
      VarN[PosN] = VarSign * VarN[PosN];           // assign sign 
      VarSign = 1;
      CallHandler();                               // Call function by first Char
    }   
    time_out_counter=0; // reset time out
    count_blinks = 0;
  }
}

void ResetReadBuff(){
    VarHandlerChar='@';
    SerIn='@';
    VarSign=1;
    PosN=-1;
}

void CallHandler(void) {
  //DebugOut();
  switch(VarHandlerChar) {
  case 'D': // set digital D 1 2 1 ! - write pin 2 HIGH, D 0 2 ! - read pin 2
    DigitalHandler(VarN[0],VarN[1],VarN[2]);
    break;    
  case 'B':  // 
    DebugOut();
    break;
  case 'A':   // analogWrite
    AnalogHandler(VarN[0],VarN[1],VarN[2]);
    break;
  case 'S':  // servo control
    ServoHandler(VarN[0],VarN[1],VarN[2],VarN[3]);
    break;    
  case 'C': // configure pin
    ConfigurePinHandler(VarN[0],VarN[1]);
    break;
  case 'I': // init start state
    InitStateHandler();
    break;
  default:
    Serial.println('0');
  } // Switch END
}

void DebugOut(void) {
  Serial.print(VarHandlerChar, BYTE);
  for (int  i = 0; i < PosN; i++) {
    Serial.print(' ');
    Serial.print(VarN[i]);
  }
  Serial.println(" !");
}

void InitStateHandler(){
    int i;
    for (i=0;i<serv_size;i++){
        if(serv[i].attached()){
            serv[i].detach();
        }
    }
    for (i=0;i<13; i++){
        pinMode(i,INPUT);
        digitalWrite(i,LOW);
    }
    Serial.println("0");
}

void ConfigurePinHandler(int pin_, int value_){
    if(value_<=0){
        pinMode(pin_,INPUT);
    }else{
        pinMode(pin_,OUTPUT);
    }
    Serial.println("0");
}

void DigitalHandler(int mode_, int pin_, int value_){
    if(mode_<=0){ //read
        Serial.println(digitalRead(pin_));
    }else{
        if(value_<=0){
            digitalWrite(pin_,LOW);
        }else{
            digitalWrite(pin_,HIGH);
        }
        Serial.println("0");
    }
}

void AnalogHandler(int mode_, int pin_, int value){
     if(mode_<=0){ //read
        Serial.println(analogRead(pin_));
    }else{
        analogWrite(pin_,value);
        Serial.println("0");
    }
}

void ServoHandler(int mode_, int number, int pin_, int value_){
    // Servo
    if(number>=serv_size){
        Serial.println("1");
        return;
    }
    switch(mode_){
    case 5: // attach()
      //add servo
      if(serv[number].attached()){
          serv[number].detach();
          serv[number].attach(pin_);
      }else{
          serv[number].attach(pin_);
      }
      Serial.println("0");
      break;
    case 6: // write()
      serv[number].write(value_);
      Serial.println("0");
      break;
    case 7: // read()
      Serial.println(serv[number].read());
      break;
    case 8: // attached()
      Serial.println(serv[number].attached());
      break;
    case 9: // dettach()
      if(serv[number].attached()){
        serv[number].detach();
      }
      Serial.println("0");
      break;
    }
}

void setup() {
  Serial.begin(9600);
  Serial.println(F_CPU);
}

void loop(){
    SerialParser();
    
    if ( time_out_counter>F_CPU/500){ // for response when time out call handler
        time_out_counter=0;
        //Serial.println("0");
        ResetReadBuff();
        count_blinks++;
    }
    if(count_blinks < max_count_blinks){
        time_out_counter++;
    }
}
